from microbit import *

NA = 0x10

def resetDongLi():
    dongLi.set_motors(5,0)
 

class N(object):
    def __init__(self):
        i2c.init()

    def set_motors(self, motor, speed):
        if motor == 5:
            for x in range(1, 5):
                self.set_motors(x, speed)
            return
        v = round(speed)
        try:
            if v < 0:
                i2c.write(NA, bytearray([motor, 0x02, v * -1, 0]))
            else:
                i2c.write(NA, bytearray([motor, 0x01, v, 0]))
        except:
            pass

    def stop_motors(self, motor):
        self.set_motors(motor, 0)

    def set_servo(self, servo, angle):
        v = round(angle/2)
        try:
            i2c.write(NA, bytearray([0x10 + servo - 1, v, 0, 0]))
        except:
            pass

dongLi = N()
resetDongLi()